#include <Movement.h>
#include <Ping.h>
int horz = A2;
int vert = A1;
int RM_EN = 10;
int LM_EN = 11;
int readVal,hVal,vVal;
int LFWD = 6;
int LREV = 7;
int RFWD = 8;
int RREV = 9;
boolean debug = false;
Movement mover(LFWD,LREV,RFWD,RREV,LM_EN,RM_EN);
Ping ping = Ping(4,74,29); 
void setup() 
{ 
  if(debug){
    Serial.begin(9600);
  }
} 
 
 
void loop() 
{ 
  readVal = analogRead(horz);
  if(debug){
    //5v board
    hVal = map(readVal,3,562,100,0);
  }else{
    //3.3v board
    hVal = map(readVal,3,850,100,0);
  }
  if(hVal > 100)
    hVal = 100;
  if(hVal < 0)
    hVal = 0;
  readVal = analogRead(vert);
  if(debug){
    //5v board
    vVal = map(readVal,2,560,0,100);
  }else{
    //3.3v board
    vVal = map(readVal,3,850,0,100);
  }
  if(vVal > 100)
    vVal = 100;
  if(vVal < 0)
    vVal = 0;
  ping.fire();
  if(ping.inches() > 3){
    mover.setMove(hVal,vVal);
  }else if(vVal >= 50-mover.deadzone){
    mover.setMove(hVal,vVal);
  }else{
    mover.setMove(50,50);
  }
  if(debug){
    delay(200);
  }else{
    delay(10); 
  }
} 
